#include "esp_log.h"
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "stdint.h"
#include "stdbool.h"
#include "driver/gpio.h"
#include "adxl345.h"
#include "i2c.h"
#include "math.h"

#define ADXL345_IIC_SDA     GPIO_NUM_16
#define ADXL345_IIC_SCL     GPIO_NUM_17
#define ACC (0x53)//(0xA7>>1)
#define A_TO_READ (6)
static const char *TAG = "adxl345";

// Write val to address register on ACC
void writeTo(uint8_t DEVICE, uint8_t address, uint8_t val) {
	if (!i2c_slave_write_with_reg(DEVICE, address, val)) {
		ESP_LOGE(TAG, "I2C write error");
	}
}

// Read num bytes starting from address register on ACC in to buff array
void readFrom(uint8_t DEVICE, uint8_t address, uint8_t num, uint8_t buff[]) {
	if (!i2c_slave_read(DEVICE, address, buff, num)) {
		ESP_LOGE(TAG, "I2C read error");
	}
}

void ADXL345_init(void) {
	// Turning on ADXL345
	i2c_init(ADXL345_IIC_SCL, ADXL345_IIC_SDA);
	writeTo(ACC, 0x2D, 1 << 3);
	writeTo(ACC, 0x31, 0x0B);
	writeTo(ACC, 0x2C, 0x09);
}

void getAccelerometerData(int *result) {
	uint8_t regAddress = 0x32;
	uint8_t buff[A_TO_READ];
	readFrom(ACC, regAddress, A_TO_READ, buff);
	result[0] = (((int) buff[1]) << 8) | buff[0];
	result[1] = (((int) buff[3]) << 8) | buff[2];
	result[2] = (((int) buff[5]) << 8) | buff[4];
}
/**
  * @brief  读取三轴数据
  * @param  x: x轴数据buffer地址
  * @param  y: y轴数据buffer地址
  * @param  z: z轴数据buffer地址
  * @retval None
  */
void ADXL345_readData(short *x, short *y, short *z)
{
    uint8_t buffer[6];
    uint8_t data;
    readFrom(ACC, 0x32, A_TO_READ, buffer);
	*x=(short)(((uint16_t)buffer[1] << 8) + buffer[0]); // 合成数据
	*y=(short)(((uint16_t)buffer[3] << 8) + buffer[2]);
	*z=(short)(((uint16_t)buffer[5] << 8) + buffer[4]);
}

/**
  * @brief  连读读取几次取平均值
  * @param  None
  * @retval None
  */
void ADXL345_readAverage(short *x, short *y, short *z, uint8_t times)
{
	short tempX, tempY, tempZ;
	*x=0;
	*y=0;
	*z=0;
	if(times)//读取次数不为0
	{
		for(uint8_t i = 0; i < times; i++)//连续读取times次
		{
			ADXL345_readData(&tempX, &tempY, &tempZ);
			*x+=tempX;
			*y+=tempY;
			*z+=tempZ;
			vTaskDelay(5 / portTICK_PERIOD_MS);
		}
		*x /= times;
		*y /= times;
		*z /= times;
	}
}
/**
  * @brief  计算角度
  * @param  None
  * @retval None
  */
void ADXL345_readAngle(float *xAngle, float *yAngle, float *zAngle)
{
	short ax,ay,az;
	ADXL345_readAverage(&ax,&ay,&az,10);
	*xAngle=atan(ax/sqrt((az*az+ay*ay)))*180/3.14;
	*yAngle=atan(ay/sqrt((ax*ax+az*az)))*180/3.14;
	*zAngle=atan(sqrt((ax*ax+ay*ay)/az))*180/3.14;
}
